
// import oimo.common.Setting;
// import oimo.dynamics.TimeStep;
// import oimo.m.Mat3;
// import oimo.m.Quat;
// import oimo.m.Vec3;
// import oimo.m.M;
// import oimo.common.MathUtil;
// import oimo.common.Vec3;
// import oimo.dynamics.*;
// import oimo.common.Transform;
// import oimo.dynamics.constraint.info.JacobianRow;
// import oimo.dynamics.constraint.info.joint.JointSolverInfo;
// import oimo.dynamics.constraint.info.joint.JointSolverInfoRow;

import { Joint } from "./Joint";
import { SpringDamper } from "./SpringDamper";
import { RotationalLimitMotor } from "./RotationalLimitMotor";
import { Vec3 } from "../../../common/Vec3";

import { RagdollJointConfig } from "./RagdollJointConfig";
import { JointType } from "./JointType";
import { M } from "../../../common/M";
import { Setting } from "../../../common/Setting";
import { JointSolverInfo } from "../info/joint/JointSolverInfo";
import { TimeStep } from "../../TimeStep";
import { Mat3 } from "../../../common/Mat3";
import { JointSolverInfoRow } from "../info/joint/JointSolverInfoRow";
import { JacobianRow } from "../info/JacobianRow";
import { MathUtil } from "../../../common/MathUtil";
import { Transform } from "../../../common/Transform";
import { Quat } from "../../../common/Quat";

/**
 * A ragdoll joint is designed to simulate ragdoll's limbs. It constrains
 * swing and twist angles between two rigid bodies. The two rigid bodies
 * have constraint axes, and the swing angle is defined by the angle of
 * two constraint axes, while the twist angle is defined by the rotation
 * angle along the two axes. In addition to lower and upper limits of the
 * twist angle, You can set an "elliptic cone limit" of the swing angle
 * by specifying two swing axes (though one of them is automatically
 * computed) and corresponding maximum swing angles. You can also enable a
 * motor of the twist part of the constraint, spring and damper effect of
 * the both swing and twist part of the constraint.
 */

export class RagdollJoint extends Joint {
	_twistSd: SpringDamper;
	_swingSd: SpringDamper;
	_twistLm: RotationalLimitMotor;
	_maxSwingAngle1: number;
	_maxSwingAngle2: number;

	swingAxis: Vec3;
	twistAxis: Vec3;

	linearError: Vec3;
	swingError: number;
	dummySwingLm: RotationalLimitMotor;

	_swingAngle: number;
	_twistAngle: number;

	/**
	 * Creates a new ragdoll joint by configuration `config`.
	 */
	constructor(config: RagdollJointConfig) {
		super(config, JointType.RAGDOLL);

		M.vec3_fromVec3(this._localBasisX1, config.localTwistAxis1);
		M.vec3_fromVec3(this._localBasisY1, config.localSwingAxis1);
		M.vec3_fromVec3(this._localBasisX2, config.localTwistAxis2);

		this.buildLocalBasesFromXY1X2();

		this._twistSd = config.twistSpringDamper.clone();
		this._twistLm = config.twistLimitMotor.clone();
		this._swingSd = config.swingSpringDamper.clone();
		this._maxSwingAngle1 = config.maxSwingAngle1;
		this._maxSwingAngle2 = config.maxSwingAngle2;

		if (this._maxSwingAngle1 < Setting.minRagdollMaxSwingAngle) {
			this._maxSwingAngle1 = Setting.minRagdollMaxSwingAngle;
		}
		if (this._maxSwingAngle2 < Setting.minRagdollMaxSwingAngle) {
			this._maxSwingAngle2 = Setting.minRagdollMaxSwingAngle;
		}

		this.dummySwingLm = new RotationalLimitMotor();
		this.dummySwingLm.lowerLimit = -1;
		this.dummySwingLm.upperLimit = 0;

		this._swingAngle = 0;
		this._twistAngle = 0;
		this.swingError = 0;
		M.vec3_zero(this.swingAxis);
		M.vec3_zero(this.twistAxis);
	}

	// --- ---

	public getInfo(info: JointSolverInfo, timeStep: TimeStep, isPositionPart: boolean): void {
		// compute ERP
		var erp: number = this.getErp(timeStep, isPositionPart);

		// compute rhs
		var linearRhs: Vec3 = new Vec3();
		M.vec3_scale(linearRhs, this.linearError, erp);
		var linRhsX: number = M.vec3_get(linearRhs, 0);
		var linRhsY: number = M.vec3_get(linearRhs, 1);
		var linRhsZ: number = M.vec3_get(linearRhs, 2);

		var crossR1: Mat3 = new Mat3();
		var crossR2: Mat3 = new Mat3();
		M.vec3_toCrossMatrix(crossR1, this._relativeAnchor1);
		M.vec3_toCrossMatrix(crossR2, this._relativeAnchor2);
		M.mat3_negate(crossR1, crossR1);
		M.mat3_negate(crossR2, crossR2);

		var row: JointSolverInfoRow;
		var j: JacobianRow;
		var swingMass: number = (this.computeEffectiveInertiaMoment(this.swingAxis));
		var twistMass: number = (this.computeEffectiveInertiaMoment(this._basisX2));

		// linear X
		row = info.addRow(this._impulses[0]);
		row.equalLimit(linRhsX, 0);

		j = row.jacobian;
		M.vec3_set(j.lin1, 1, 0, 0);
		M.vec3_set(j.lin2, 1, 0, 0);
		M.mat3_getRow(j.ang1, crossR1, 0);
		M.mat3_getRow(j.ang2, crossR2, 0);

		// linear Y
		row = info.addRow(this._impulses[1]);
		row.equalLimit(linRhsY, 0);

		j = row.jacobian;
		M.vec3_set(j.lin1, 0, 1, 0);
		M.vec3_set(j.lin2, 0, 1, 0);
		M.mat3_getRow(j.ang1, crossR1, 1);
		M.mat3_getRow(j.ang2, crossR2, 1);

		// linear Z
		row = info.addRow(this._impulses[2]);
		row.equalLimit(linRhsZ, 0);

		j = row.jacobian;
		M.vec3_set(j.lin1, 0, 0, 1);
		M.vec3_set(j.lin2, 0, 0, 1);
		M.mat3_getRow(j.ang1, crossR1, 2);
		M.mat3_getRow(j.ang2, crossR2, 2);

		// swing
		if (this.swingError > 0 && (this._swingSd.frequency <= 0 || !isPositionPart)) {
			row = info.addRow(this._impulses[3]);
			this.setSolverInfoRowAngular(row, this.swingError, this.dummySwingLm, swingMass, this._swingSd, timeStep, isPositionPart);

			j = row.jacobian;
			M.vec3_assign(j.ang1, this.swingAxis);
			M.vec3_assign(j.ang2, this.swingAxis);
		}

		// twist
		if (this._twistSd.frequency <= 0 || !isPositionPart) {
			row = info.addRow(this._impulses[4]);
			this.setSolverInfoRowAngular(row, this._twistAngle, this._twistLm, twistMass, this._twistSd, timeStep, isPositionPart);

			j = row.jacobian;

			M.vec3_assign(j.ang1, this.twistAxis);
			M.vec3_assign(j.ang2, this.twistAxis);
		}
	}


	private computeErrors(): void {
		var tf1: Transform = this._b1._transform;
		var tf2: Transform = this._b2._transform;

		// twist axes
		var axis1: Vec3 = new Vec3();
		var axis2: Vec3 = new Vec3();
		M.vec3_assign(axis1, this._basisX1);
		M.vec3_assign(axis2, this._basisX2);

		// build basis matrices
		var basis1Mat: Mat3 = new Mat3();
		var basis2Mat: Mat3 = new Mat3();
		M.mat3_fromCols(basis1Mat, this._basisX1, this._basisY1, this._basisZ1);
		M.mat3_fromCols(basis2Mat, this._basisX2, this._basisY2, this._basisZ2);

		// compute the swing matrix
		var swingQ: Quat = new Quat();
		var swingM: Mat3 = new Mat3();
		var swingV: Vec3 = new Vec3();
		M.quat_arc(swingQ, axis1, axis2);
		M.mat3_fromQuat(swingM, swingQ);

		// get swing angle
		this._swingAngle = MathUtil.safeAcos(M.quat_getReal(swingQ)) * 2;
		M.vec3_fromQuat(swingV, swingQ);

		// swing basisY2 into rb1's coordinates
		var basisY2In1: Vec3 = new Vec3();
		M.vec3_mulMat3Transposed(basisY2In1, this._basisY2, swingM);

		// compute twist angle
		var yCoord: number;
		var zCoord: number;
		yCoord = M.vec3_dot(this._basisY1, basisY2In1);
		zCoord = M.vec3_dot(this._basisZ1, basisY2In1);
		this._twistAngle = MathUtil.atan2(zCoord, yCoord);

		// compute twist axis: middle vector between basis1X and basis2X
		M.vec3_add(this.twistAxis, this._basisX1, this._basisX2);
		M.vec3_normalize(this.twistAxis, this.twistAxis);

		// scale the swing vector so that its length shows the swing rotation angle
		var invLen: number = M.vec3_length(swingV);
		if (invLen > 0) invLen = 1 / invLen;
		M.vec3_scale(swingV, swingV, invLen * this._swingAngle);

		// take the swing axis to the first body's constraint coordinate system
		M.vec3_mulMat3Transposed(swingV, swingV, basis1Mat);

		// project the swing rotation angles onto XY plane
		var x: number = M.vec3_get(swingV, 1);
		var y: number = M.vec3_get(swingV, 2);

		// constraint ellipse: x^2/a^2 + y^2/b^2 <= 1
		var a: number = this._maxSwingAngle1;
		var b: number = this._maxSwingAngle2;
		var invA2: number = 1 / (a * a);
		var invB2: number = 1 / (b * b);

		var w: number = x * x * invA2 + y * y * invB2;
		if (w == 0) {
			M.vec3_set(this.swingAxis, 0, 0, 0);
			this.swingError = 0;
		} else {
			var t: number = MathUtil.sqrt(1 / w);
			var x0: number = x * t;
			var y0: number = y * t;
			var nx: number = x0 * invA2;
			var ny: number = y0 * invB2;
			invLen = 1 / MathUtil.sqrt(nx * nx + ny * ny);
			nx *= invLen;
			ny *= invLen;
			var depth: number = (x - x0) * nx + (y - y0) * ny;
			if (depth > 0) {
				this.swingError = depth;

				// normal vector in constraint ellipse space
				M.vec3_set(this.swingAxis, 0, nx, ny);
				// take it to the first body's space
				M.vec3_mulMat3(this.swingAxis, this.swingAxis, basis1Mat);
				// then swing it
				M.vec3_mulMat3(this.swingAxis, this.swingAxis, swingM);
			} else {
				this.swingError = 0;
			}
		}

		// compute linear error
		M.vec3_sub(this.linearError, this._anchor2, this._anchor1);
	}

	// --- internal ---

	public _syncAnchors(): void {
		super._syncAnchors();

		// compute positional errors
		this.computeErrors();
	}

	public _getVelocitySolverInfo(timeStep: TimeStep, info: JointSolverInfo): void {
		super._getVelocitySolverInfo(timeStep, info);
		this.getInfo(info, timeStep, false);
	}

	public _getPositionSolverInfo(info: JointSolverInfo): void {
		super._getPositionSolverInfo(info);
		this.getInfo(info, null, true);
	}

	// --- public ---

	/**
	 * Returns the first rigid body's constraint axis in world coordinates.
	 */
	public getAxis1(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._basisX1);
		return v;
	}

	/**
	 * Returns the second rigid body's constraint axis in world coordinates.
	 */
	public getAxis2(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._basisX2);
		return v;
	}

	/**
	 * Sets `axis` to the first rigid body's constraint axis in world coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAxis1To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._basisX1);
	}

	/**
	 * Sets `axis` to the second rigid body's constraint axis in world coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getAxis2To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._basisX2);
	}

	/**
	 * Returns the first rigid body's constraint axis relative to the rigid body's transform.
	 */
	public getLocalAxis1(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._localBasisX1);
		return v;
	}

	/**
	 * Returns the second rigid body's constraint axis relative to the rigid body's transform.
	 */
	public getLocalAxis2(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._localBasisX2);
		return v;
	}

	/**
	 * Sets `axis` to the first rigid body's constraint axis relative to the rigid body's transform.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalAxis1To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._localBasisX1);
	}

	/**
	 * Sets `axis` to the second rigid body's constraint axis relative to the rigid body's transform.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalAxis2To(axis: Vec3): void {
		M.vec3_toVec3(axis, this._localBasisX2);
	}

	/**
	 * Returns the rotational spring and damper settings along the twist axis.
	 */
	public getTwistSpringDamper(): SpringDamper {
		return this._twistSd;
	}

	/**
	 * Returns the rotational limits and motor settings along the twist axis.
	 */
	public getTwistLimitMotor(): RotationalLimitMotor {
		return this._twistLm;
	}

	/**
	 * Returns the rotational spring and damper settings along the swing axis.
	 */
	public getSwingSpringDamper(): SpringDamper {
		return this._swingSd;
	}

	/**
	 * Returns the swing axis in world coordinates.
	 */
	public getSwingAxis(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this.swingAxis);
		return v;
	}

	/**
	 * Sets `axis` to the swing axis in world coordinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getSwingAxisTo(axis: Vec3): void {
		M.vec3_toVec3(axis, this.swingAxis);
	}

	/**
	 * Returns the swing angle in radians.
	 */
	public getSwingAngle(): number {
		return this._swingAngle;
	}

	/**
	 * Returns the twist angle in radians.
	 */
	public getTwistAngle(): number {
		return this._twistAngle;
	}

}
